/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * File Name          : freertos.c
 * Description        : Code for freertos applications
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
 * All rights reserved.</center></h2>
 *
 * This software component is licensed by ST under Ultimate Liberty license
 * SLA0044, the "License"; You may not use this file except in compliance with
 * the License. You may obtain a copy of the License at:
 *                             www.st.com/SLA0044
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#ifdef __cplusplus
extern "C" {
#endif
#include "oslib.h"
#include "usart.h"
#include "../UserFunc/Dog.h"
#include "../UserFunc/imu.h"
#include "../UserFunc/HX711.h"
#include "../UserFunc/state.h"
#include "../UserFunc/valuepack.h"
#include "../UserFunc/flash.h"
#ifdef __cplusplus
}
#endif
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for MotionlCtrlTask */
osThreadId_t MotionlCtrlTaskHandle;
const osThreadAttr_t MotionlCtrlTask_attributes = {
  .name = "MotionlCtrlTask",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityAboveNormal,
};
/* Definitions for SensorTask */
osThreadId_t SensorTaskHandle;
const osThreadAttr_t SensorTask_attributes = {
  .name = "SensorTask",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityHigh,
};
/* Definitions for VisionTask */
osThreadId_t VisionTaskHandle;
const osThreadAttr_t VisionTask_attributes = {
  .name = "VisionTask",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for RemoteTask */
osThreadId_t RemoteTaskHandle;
const osThreadAttr_t RemoteTask_attributes = {
  .name = "RemoteTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityHigh4,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);
void StartMotionCtrl(void *argument);
void StartSensorTask(void *argument);
void StartVisionTask(void *argument);
void StartRemoteTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */
  OSLIB_Init();
  // FLASH_Init();
  usart6_init();
  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* creation of MotionlCtrlTask */
  MotionlCtrlTaskHandle = osThreadNew(StartMotionCtrl, NULL, &MotionlCtrlTask_attributes);

  /* creation of SensorTask */
  SensorTaskHandle = osThreadNew(StartSensorTask, NULL, &SensorTask_attributes);

  /* creation of VisionTask */
  VisionTaskHandle = osThreadNew(StartVisionTask, NULL, &VisionTask_attributes);

  /* creation of RemoteTask */
  RemoteTaskHandle = osThreadNew(StartRemoteTask, NULL, &RemoteTask_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
 * @brief  Function implementing the defaultTask thread.
 * @param  argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  uprintf("StartDefaultTask\n\r");
  /* Infinite loop */
  for (;;)
  {
    HAL_GPIO_TogglePin(LED3_GPIO_Port, LED3_Pin);
//    show_broadcast();
    osDelay(500);
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartMotionCtrl */
/**
 * @brief Function implementing the MotionlCtrlTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartMotionCtrl */
void StartMotionCtrl(void *argument)
{
  /* USER CODE BEGIN StartMotionCtrl */
  Dog_init();
  /* Infinite loop */
  for (;;)
  {
    Dog_exe();

    osDelay(2);
  }
  /* USER CODE END StartMotionCtrl */
}

/* USER CODE BEGIN Header_StartSensorTask */
/**
 * @brief Function implementing the SensorTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartSensorTask */
void StartSensorTask(void *argument)
{
  /* USER CODE BEGIN StartSensorTask */
  uprintf("imu task\n\r");
  UART3_TaskBufferSize = 256;
  // osSemaphoreAcquire(UART3_RxSema, osWaitForever);
  /* Infinite loop */
  for (;;)
  {
//     Weight_exe();
    // uprintf("F0 %d | F1 %d | F2 %d | F3 %d\n\r", Weight_Shiwu[0], Weight_Shiwu[1], Weight_Shiwu[2], Weight_Shiwu[3]);
    // imu_exe();
    osDelay(2);
  }
  /* USER CODE END StartSensorTask */
}

/* USER CODE BEGIN Header_StartVisionTask */
/**
 * @brief Function implementing the VisionTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartVisionTask */
void StartVisionTask(void *argument)
{
  /* USER CODE BEGIN StartVisionTask */

  /* Infinite loop */
  // uint8_t data = 0;
  for (;;)
  {
    // if (HAL_GPIO_ReadPin(S1_GPIO_Port, S1_Pin) == GPIO_PIN_SET && state == state_debug)
    // {
    //   Dog.ctrl_mode = DOG_MODE_RESET;
    //   osDelay(187);
    //   Dog.ctrl_mode = DOG_MODE_WALK;
    //   state = state_1_begin;
    //   uprintf("set state 1 success\n\r");
    // }
    state_machine_exe();
    osDelay(10);
  }
  /* USER CODE END StartVisionTask */
}

/* USER CODE BEGIN Header_StartRemoteTask */
/**
 * @brief Function implementing the RemoteTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_StartRemoteTask */
void StartRemoteTask(void *argument)
{
  /* USER CODE BEGIN StartRemoteTask */

  // initValuePack();

  /* Infinite loop */
  for (;;)
  {
    // uprintf("recvive time %.1f\n\r", osKernelGetTickCount() / 1000.0);
    // bluetooth_handle_exe();
    osDelay(2);
  }
  /* USER CODE END StartRemoteTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */

